#include "dtm.h"
#include "app_timer.h"
#include <stdbool.h>
#include <stdio.h>


static struct testmode_config cfg;

#define T_MIN 6  // Inter byte timeout is 5 msec in spec, add 1 msec for tolerance

uint8_t *pktarry[4] = {	"PRBS9 packet payload(test_pkt_prbs9)",
						"11110000 packet payload(test_pkt_11110000)",
						"10101010 packet payload(test_pkt_10101010)",
						"Unmodulated carrier(test_pkt_carrier)"};

static struct
{
    uint8_t data[2];
    uint8_t len;
    uint32_t last_byte_time;
} cmd_buffer;

enum cmd
{
    cmd_reset,
    cmd_rxtest,
    cmd_txtest,
    cmd_testend,
};

struct cmd_packet
{
  enum cmd cmd;
  uint8_t frequency;
  uint8_t length;
  uint8_t pkt;
};

static void reset_cmd_buffer()
{
  cmd_buffer.len = 0;
}

static inline uint32_t t_min_in_ticks()
{
  return T_MIN * cfg.ticks_per_second / 1000;
}

void testmode_init(const struct testmode_config *config)
{
  cfg = *config;
  reset_cmd_buffer();
}

void testmode_process_command_byte(uint8_t byte)
{
  if (cmd_buffer.len >= sizeof(cmd_buffer.data))
  {
    // Processing previous command => ignore byte
    return;
  }

  uint32_t current_byte_time = cfg.get_ticks();

  if (cmd_buffer.len &&
      current_byte_time - cmd_buffer.last_byte_time > t_min_in_ticks())
  {
    // Inter byte timeout occurred
    reset_cmd_buffer();
  }

  cmd_buffer.last_byte_time = current_byte_time;
  cmd_buffer.data[cmd_buffer.len++] = byte;

  if (cmd_buffer.len == sizeof(cmd_buffer.data))
  {
    gecko_external_signal(cfg.command_ready_signal);
  }
}

static void parse_cmd_buffer(struct cmd_packet *result)
{
  result->cmd = (enum cmd)(cmd_buffer.data[0] >> 6);
  result->frequency = cmd_buffer.data[0] & 63;
  result->length = cmd_buffer.data[1] >> 2;
  result->pkt = cmd_buffer.data[1] & 0x3;
}

static void process_command()
{
  struct cmd_packet cmd;
  parse_cmd_buffer(&cmd);

  switch (cmd.cmd)
  {
    case cmd_reset:
    	printf("gecko_cmd_test_dtm_reset\r\n");
    	gecko_cmd_test_dtm_end();
      break;

    case cmd_rxtest:
    	printf("gecko_cmd_test_dtm_rx frequency = %d MHz\r\n",cmd.frequency * 2 + 2402);
    	gecko_cmd_test_dtm_rx(cmd.frequency);
      break;

    case cmd_txtest:
    	printf("gecko_cmd_test_dtm_tx pkt = %s length = %d frequency = %d MHz\r\n",pktarry[cmd.pkt], cmd.length,cmd.frequency * 2 + 2402);
    	gecko_cmd_test_dtm_tx(cmd.pkt, cmd.length, cmd.frequency);
      break;

    case cmd_testend:
    	printf("gecko_cmd_test_dtm_end\r\n");
    	gecko_cmd_test_dtm_end();
      break;
  }
}

static void send_test_status(uint8 error)
{
  cfg.write_response_byte(0);
  cfg.write_response_byte(error ? 1 : 0);
}

static void send_packet_counter(uint16 counter)
{
  counter |= 0x8000;  // EV bit on
  cfg.write_response_byte(counter >> 8);
  cfg.write_response_byte(counter & 0xff);
}

static void handle_dtm_completed(struct gecko_cmd_packet *evt)
{
  struct cmd_packet cmd;
  parse_cmd_buffer(&cmd);

  if (cmd.cmd == cmd_testend) {
    send_packet_counter(evt->data.evt_test_dtm_completed.number_of_packets);
  } else {
    send_test_status(evt->data.evt_test_dtm_completed.result);
  }

  // Command is executed and the response is sent => reset command buffer for next command
  reset_cmd_buffer();
}

int testmode_handle_gecko_event(struct gecko_cmd_packet *evt)
{
	static uint8_t i = 0,j = 0, m = 0,n = 0;
  switch (BGLIB_MSG_ID(evt->header))
  {
  	  case gecko_evt_system_boot_id:
  		  printf("system boot id\r\n");
  		  gecko_cmd_system_set_tx_power(190);
  		  gecko_cmd_hardware_set_soft_timer(TIMER_MS_2_TIMERTICK(20),KEYDET,false);
  		break;
    case gecko_evt_system_external_signal_id:
      if (evt->data.evt_system_external_signal.extsignals & cfg.command_ready_signal)
      {
        process_command();
        return 1;
      }
      break;

    case gecko_evt_test_dtm_completed_id:
      handle_dtm_completed(evt);
      return 1;
      break;

    case gecko_evt_hardware_soft_timer_id:
		/* Check which software timer handle is in question */
		switch (evt->data.evt_hardware_soft_timer.handle)
		{
			case KEYDET:

				if(!GPIO_PinInGet(keyport,keypin))//F2
				{
					while(!GPIO_PinInGet(keyport,keypin));//F2
					//printf("key det %d\r\n",i);
					if(i == 1)//reset
					{
						j = m = n = 0;
						gecko_cmd_hardware_set_soft_timer(TIMER_MS_2_TIMERTICK(0),TXTEST,true);
						gecko_cmd_hardware_set_soft_timer(TIMER_MS_2_TIMERTICK(0),RXTEST,true);
						cmd_buffer.data[0] = 0x00;
						cmd_buffer.data[1] = 0x00;
						process_command();
					}
					else if(i == 2)	//rx
					{
						gecko_cmd_hardware_set_soft_timer(TIMER_MS_2_TIMERTICK(0),TXTEST,true);
						gecko_cmd_hardware_set_soft_timer(TIMER_MS_2_TIMERTICK(5000),RXTEST,false);
					}
					else if(i == 3)//tx
					{
						gecko_cmd_hardware_set_soft_timer(TIMER_MS_2_TIMERTICK(0),RXTEST,true);
						gecko_cmd_hardware_set_soft_timer(TIMER_MS_2_TIMERTICK(5000),TXTEST,false);
					}
					else if(i == 4)//end
					{
						gecko_cmd_hardware_set_soft_timer(TIMER_MS_2_TIMERTICK(0),TXTEST,true);
						gecko_cmd_hardware_set_soft_timer(TIMER_MS_2_TIMERTICK(0),RXTEST,true);
						cmd_buffer.data[0] = 0xc0;
						cmd_buffer.data[1] = 0x00;
						process_command();
						j = m = n = i = 0;
					}
					i ++;
				}
			break;

			case RXTEST:
				cmd_buffer.data[0] = 0x40 | j;
				cmd_buffer.data[1] = 0x00;
				process_command();
				if((++ j) >= 40)
				{
					j = 0;
					handle_dtm_completed(evt);
				}

				break;

			case TXTEST:
				cmd_buffer.data[0] = 0x80 | m;
				cmd_buffer.data[1] = 0x94 | n;
				process_command();
				if((++ n) >= 4)
				{
					n = 0;
					if((++ m) >= 40) m = 0;
				}

				break;


			default:
			break;
		}
		break;
  }

  return 0;
}
